import rosbag
from colorama import Fore, Style
import os
import sys
import warnings
import def_function
from datetime import datetime
import json

# 获取当前脚本的完整路径
script_path = os.path.abspath(sys.argv[0])
# 从完整路径中获取目录
script_dir = os.path.dirname(script_path)
# 从完整路径中分离出文件名
script_name = os.path.basename(script_path)
# 使用 splitext() 函数分离文件名和扩展名
script_name_without_extension, extension = os.path.splitext(script_name)

# 测试用例
# https://cla.momenta.works/cdi/data/a0de6a3bafda7fc71fca368106f2793a
print(Fore.YELLOW+f"python script name:", sys.argv[0])
if os.getenv('DEBUG') == 'true':
    python_param = '/mnt/01_code/python/rosbag_analysis/dataspace/Harz_orin/PLBBSQ3142_event_dfdi_console_manual_event_20230818-111336_0_filter.bag'
else:
    if len(sys.argv)!=2:
        print(Fore.RED+f"you need python params!")
        sys.exit()
    python_param = script_dir + "/" + sys.argv[1]
    if os.path.isabs(sys.argv[1]):
        python_param = sys.argv[1]
rosbag_data = rosbag.Bag(python_param)

# 获取类型和主题信息
# rosbag_info = rosbag_data.get_type_and_topic_info()
# info.topics 是一个字典，它的键是 topic 的名字
# all_topics = list(rosbag_info.topics.keys())
topics = ['/vehicle/chassis_report', '/vehicle_service/info']

dict_msg = dict()
arry_error_info = list()
dict_error_info = dict()

chassis_report_override_brake = False
chassis_report_override_throttle = False
chassis_report_override_steering = False
chassis_report_override_gear = False

chassis_report_override_changed_true_brake = False
chassis_report_override_changed_true_throttle = False
chassis_report_override_changed_true_steering = False
chassis_report_override_changed_true_gear = False
chassis_report_override_changed_true_time_brake = 0.0
chassis_report_override_changed_true_time_throttle = 0.0
chassis_report_override_changed_true_time_steering = 0.0
chassis_report_override_changed_true_time_gear = 0.0

info_override_brake = False
info_override_throttle = False
info_override_steering = False
info_override_gear = False

cnt = 0
# 创建一个 ValueJudge 对象
dict_judge = dict()
for msg_topic, msg, t in rosbag_data.read_messages(topics=topics):
    if msg_topic == '/vehicle/chassis_report':
        chassis_report_override_brake = msg.brake_report.brake_report_data.override
        chassis_report_override_throttle = msg.throttle_report.throttle_report_data.override
        chassis_report_override_steering = msg.steering_report.steering_report_data.override
        chassis_report_override_gear = msg.gear_report.gear_report_data.override
        if msg_topic in dict_msg:
            if 1 == dict_msg[msg_topic].brake_report.brake_report_data.override and 0 == msg.brake_report.brake_report_data.override:
                chassis_report_override_changed_true_brake = True
                chassis_report_override_changed_true_time_brake = t.to_sec()
            if 1 == dict_msg[msg_topic].throttle_report.throttle_report_data.override and 0 == msg.throttle_report.throttle_report_data.override:
                chassis_report_override_changed_true_throttle = True
                chassis_report_override_changed_true_time_throttle = t.to_sec()
            if 1 == dict_msg[msg_topic].steering_report.steering_report_data.override and 0 == msg.steering_report.steering_report_data.override:
                chassis_report_override_changed_true_steering = True
                chassis_report_override_changed_true_time_steering = t.to_sec()
            if 1 == dict_msg[msg_topic].gear_report.gear_report_data.override and 0 == msg.gear_report.gear_report_data.override:
                chassis_report_override_changed_true_gear = True
                chassis_report_override_changed_true_time_gear = t.to_sec()
    if msg_topic == '/vehicle_service/info':
        # 使用 json.loads() 方法来解析 JSON 字符串
        parsed_json = json.loads(msg.frame_id)
        info_override_brake = parsed_json.get('brake', {}).get('status', info_override_brake)
        info_override_throttle = parsed_json.get('throttle', {}).get('status', info_override_throttle)
        info_override_steering = parsed_json.get('steering', {}).get('status', info_override_steering)
        info_override_gear = parsed_json.get('gear', {}).get('status', info_override_gear)
    
    if True == info_override_brake:
        chassis_report_override_changed_true_brake = False
    if True == info_override_throttle:
        chassis_report_override_changed_true_throttle = False
    if True == info_override_steering:
        chassis_report_override_changed_true_steering = False
    if True == info_override_gear:
        chassis_report_override_changed_true_gear = False


    error_type = "刹车同步"
    if dict_error_info.get(error_type) is None:
        dict_error_info.setdefault(error_type, def_function.class_data.error_info())
    err_info = dict_error_info[error_type]
    keep_time = 0.2
    judge_condition = "/vehicle/chassis_report与/vehicle_service/info不同步持续200ms"
    if dict_judge.get(error_type) is None:
        dict_judge[error_type] = def_function.judge_value.ValueJudge()
    result = dict_judge[error_type].timeout(t.to_sec(), chassis_report_override_brake, info_override_brake, keep_time, "!=");
    message = "[" + error_type + "{" + judge_condition + "}]" +  " /vehicle/chassis_report:" + str(chassis_report_override_brake) + " vehicle_service/info:" + str(info_override_brake)
    def_function.class_data.update_error_info(t.to_sec(), result, message, dict_error_info, error_type, arry_error_info)

    error_type = "刹车变化"
    if dict_error_info.get(error_type) is None:
        dict_error_info.setdefault(error_type, def_function.class_data.error_info())
    err_info = dict_error_info[error_type]
    keep_time = 0.2
    judge_condition = "/vehicle/chassis_report变化为True后/vehicle_service/info未变化为True持续200ms"
    if dict_judge.get(error_type) is None:
        dict_judge[error_type] = def_function.judge_value.ValueJudge()
    judge_result = True == chassis_report_override_changed_true_brake and False == info_override_brake
    result = dict_judge[error_type].timeout(t.to_sec(), judge_result, True, keep_time, "==")
    message = "[" + error_type + "{" + judge_condition + "}]" +  " /vehicle/chassis_report:" + str(chassis_report_override_changed_true_brake) + " vehicle_service/info:" + str(info_override_brake)
    def_function.class_data.update_error_info(t.to_sec(), result, message, dict_error_info, error_type, arry_error_info)

    error_type = "油门同步"
    if dict_error_info.get(error_type) is None:
        dict_error_info.setdefault(error_type, def_function.class_data.error_info())
    err_info = dict_error_info[error_type]
    keep_time = 0.2
    judge_condition = "/vehicle/chassis_report与/vehicle_service/info不同步持续200ms"
    if dict_judge.get(error_type) is None:
        dict_judge[error_type] = def_function.judge_value.ValueJudge()
    result = dict_judge[error_type].timeout(t.to_sec(), chassis_report_override_throttle, info_override_throttle, keep_time, "!=");
    message = "[" + error_type + "{" + judge_condition + "}]" +  " /vehicle/chassis_report:" + str(chassis_report_override_throttle) + " vehicle_service/info:" + str(info_override_throttle)
    def_function.class_data.update_error_info(t.to_sec(), result, message, dict_error_info, error_type, arry_error_info)

    error_type = "油门变化"
    if dict_error_info.get(error_type) is None:
        dict_error_info.setdefault(error_type, def_function.class_data.error_info())
    err_info = dict_error_info[error_type]
    keep_time = 0.2
    judge_condition = "/vehicle/chassis_report变化为True后/vehicle_service/info未变化为True持续200ms"
    if dict_judge.get(error_type) is None:
        dict_judge[error_type] = def_function.judge_value.ValueJudge()
    judge_result = True == chassis_report_override_changed_true_throttle and False == info_override_throttle
    result = dict_judge[error_type].timeout(t.to_sec(), judge_result, True, keep_time, "==")
    message = "[" + error_type + "{" + judge_condition + "}]" +  " /vehicle/chassis_report:" + str(chassis_report_override_changed_true_throttle) + " vehicle_service/info:" + str(info_override_throttle)
    def_function.class_data.update_error_info(t.to_sec(), result, message, dict_error_info, error_type, arry_error_info)

    error_type = "方向盘同步"
    if dict_error_info.get(error_type) is None:
        dict_error_info.setdefault(error_type, def_function.class_data.error_info())
    err_info = dict_error_info[error_type]
    keep_time = 0.2
    judge_condition = "/vehicle/chassis_report与/vehicle_service/info不同步持续200ms"
    if dict_judge.get(error_type) is None:
        dict_judge[error_type] = def_function.judge_value.ValueJudge()
    result = dict_judge[error_type].timeout(t.to_sec(), chassis_report_override_steering, info_override_steering, keep_time, "!=");
    message = "[" + error_type + "{" + judge_condition + "}]" +  " /vehicle/chassis_report:" + str(chassis_report_override_steering) + " vehicle_service/info:" + str(info_override_steering)
    def_function.class_data.update_error_info(t.to_sec(), result, message, dict_error_info, error_type, arry_error_info)

    error_type = "方向盘变化"
    if dict_error_info.get(error_type) is None:
        dict_error_info.setdefault(error_type, def_function.class_data.error_info())
    err_info = dict_error_info[error_type]
    keep_time = 0.2
    judge_condition = "/vehicle/chassis_report变化为True后/vehicle_service/info未变化为True持续200ms"
    if dict_judge.get(error_type) is None:
        dict_judge[error_type] = def_function.judge_value.ValueJudge()
    judge_result = True == chassis_report_override_changed_true_steering and False == info_override_steering
    result = dict_judge[error_type].timeout(t.to_sec(), judge_result, True, keep_time, "==")
    message = "[" + error_type + "{" + judge_condition + "}]" +  " /vehicle/chassis_report:" + str(chassis_report_override_changed_true_steering) + " vehicle_service/info:" + str(info_override_steering)
    def_function.class_data.update_error_info(t.to_sec(), result, message, dict_error_info, error_type, arry_error_info)
    #保存上次值
    dict_msg[msg_topic] = msg

    error_type = "档位同步"
    if dict_error_info.get(error_type) is None:
        dict_error_info.setdefault(error_type, def_function.class_data.error_info())
    err_info = dict_error_info[error_type]
    keep_time = 0.2
    judge_condition = "/vehicle/chassis_report与/vehicle_service/info不同步持续200ms"
    if dict_judge.get(error_type) is None:
        dict_judge[error_type] = def_function.judge_value.ValueJudge()
    result = dict_judge[error_type].timeout(t.to_sec(), chassis_report_override_gear, info_override_gear, keep_time, "!=");
    message = "[" + error_type + "{" + judge_condition + "}]" +  " /vehicle/chassis_report:" + str(chassis_report_override_gear) + " vehicle_service/info:" + str(info_override_gear)
    def_function.class_data.update_error_info(t.to_sec(), result, message, dict_error_info, error_type, arry_error_info)

    error_type = "档位变化"
    if dict_error_info.get(error_type) is None:
        dict_error_info.setdefault(error_type, def_function.class_data.error_info())
    err_info = dict_error_info[error_type]
    keep_time = 0.2
    judge_condition = "/vehicle/chassis_report变化为True后/vehicle_service/info未变化为True持续200ms"
    if dict_judge.get(error_type) is None:
        dict_judge[error_type] = def_function.judge_value.ValueJudge()
    judge_result = True == chassis_report_override_changed_true_gear and False == info_override_gear
    result = dict_judge[error_type].timeout(t.to_sec(), judge_result, True, keep_time, "==")
    message = "[" + error_type + "{" + judge_condition + "}]" +  " /vehicle/chassis_report:" + str(chassis_report_override_changed_true_gear) + " vehicle_service/info:" + str(info_override_gear)
    def_function.class_data.update_error_info(t.to_sec(), result, message, dict_error_info, error_type, arry_error_info)
    ################################################################################################

################################################################################################
rosbag_data.close()
list_print = list()
for value in dict_error_info.values():
    if (0 != value.start_time and 0 == value.end_time):
        list_print.append("[" + str(value.start_time) + "-" + "bag_end]" + value.message)
for item in arry_error_info:
    list_print.append("[" + str(item.start_time) + "-" + str(item.end_time) + "@diff:" + "{:.3f}".format(item.end_time - item.start_time) + "]" + item.message)
if list_print:
    print(Fore.RED+f"", script_name_without_extension)
for item in list_print:
    print(Fore.CYAN+f"", item)